In the control of motor systems, it often comes up that you would like to execute some pre-determined trajectory, like walking, running, throwing a frisbee, or handwriting.
Dynamical movement primitives (DMPs) are robust, generalizable trajectory generation systems. I give an overview of their origins and some use cases on my blog https://studywolf.wordpress.com/category/robotics/dynamic-movement-primitive/
In this tutorial, we'll be looking at a neural implementation of DMPs (NDMPs).
There are two main parts to DMPs, the point attractors and the forcing function.
For each degree-of-freedom in your movement a separate point attractor is required.
For discrete movements, the point attractor simply moves in a straight line from the starting point of the trajectory to the ending point of the trajectory.
In [ ]:
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
def point_attractor(start, target, dt=.001, alpha=400, beta=100):
x_track = [np.copy(start)]
x = np.array(start, dtype=float) # initial position
dx = np.array([0, 0], dtype=float) # initial velocity
for ii in range(30):
# ddx = alpha * (beta * (target -x)) # no velocity compensation
ddx = alpha * (beta * (target - x) - dx) # <-- point attractor dynamics
dx += ddx * dt
x += dx * dt
x_track.append(np.copy(x))
return np.array(x_track)
In [ ]:
# Discrete system point attractor
start = [0, 0] # change this and run!
end = [1, 1] # change this and run!
trajectory = point_attractor(start, end)
plt.plot(start[0], start[1], 'bx', mew=4) # blue x at start position
plt.plot(end[0], end[1], 'gx', mew=4) # green x at end position
plt.plot(trajectory[:, 0], trajectory[:, 1], '.')
plt.xlim([-2, 2])
plt.ylim([-2, 2])
For rhythmic movements, the start and end point are the same, so the point attractor just holds the system at the same position. Which, granted, is not very exciting to see. See for yourself!
In [ ]:
# Rhythmic system point attractor
start = end =[0, 0] # change this and run!
trajectory = point_attractor(start, end)
plt.plot(start[0], start[1], 'bx', mew=4) # blue x at start position
plt.plot(end[0], end[1], 'gx', mew=4) # green x at end position
plt.plot(trajectory[:, 0], trajectory[:, 1], '.')
plt.xlim([-2, 2])
plt.ylim([-2, 2])
The second part of the DMP system is the forcing function. The idea here is simply that the some additional force is added in to the point attractor dynamics, that pushes them along a path that is no longer straight as they move to the end point (discrete movements) or try to hold a position (rhythmic movements).
In [ ]:
def point_attractor_ff(start, target, ff, dt=.001, alpha=400, beta=100):
" ff is a vector of forces to apply over time "
x_track = [np.copy(start)]
x = np.array(start, dtype=float) # initial position
dx = np.array([0, 0], dtype=float) # initial velocity
for ii in range(len(ff)):
ddx = alpha * (beta * (target - x) - dx) + ff[ii]
dx += ddx * dt
x += dx * dt
x_track.append(np.copy(x))
return np.array(x_track)
In [ ]:
# Discrete system point attractor with forcing function
ff = np.vstack([
-np.sin(np.arange(0, 10, .1)),
np.cos(np.arange(0, 10, .1))]).T * 2e4
start = [0, 0] # change this and run!
end = [1, 0] # change this and run!
trajectory = point_attractor_ff(start, end, ff)
plt.plot(start[0], start[1], 'bx', mew=4) # blue x at start position
plt.plot(end[0], end[1], 'gx', mew=4) # green x at end position
plt.plot(trajectory[:, 0], trajectory[:, 1], '.')
plt.xlim([-2, 2])
plt.ylim([-2, 2])
In [ ]:
# Rhythmic system point attractor with forcing function
ff = np.vstack([
np.sin(np.arange(0, 10, .1)),
np.cos(np.arange(0, 10, .1))]).T * 2e4
start = end = [0, 0] # change this and run!
trajectory = point_attractor_ff(start, end, ff)
plt.plot(start[0], start[1], 'bx', mew=4) # blue x at start position
plt.plot(end[0], end[1], 'gx', mew=4) # green x at end position
plt.plot(trajectory[:, 0], trajectory[:, 1], '.')
plt.xlim([-2, 2])
plt.ylim([-2, 2])
In [ ]:
heart = np.load('models/handwriting_trajectories/heart.npz')['arr_0']
heart = np.vstack([heart, heart[-1]])
plt.plot(heart[:, 0], heart[:, 1], 'r--')
The forces that we apply to our point attractor affect acceleration, so what we need to do is find out what accelerations will give us the above trajectory. Let's assume our timestep is 1ms when drawing out that heart trajectory.
In [ ]:
dt = 0.001
# initial velocity is zero
d_heart = np.vstack([[0, 0], np.diff(heart, axis=0) / dt])
# final acceleration is zero
dd_heart = np.vstack([np.diff(d_heart, axis=0) / dt, [0, 0]])
plt.subplot(2, 1, 1)
plt.plot(d_heart)
plt.title('Desired velocities')
plt.subplot(2, 1, 2)
plt.plot(dd_heart)
plt.title('Desired accelerations')
plt.tight_layout()
So if there were no other forces affecting the system, those are the forces that we would need to apply to draw out a heart. HOWEVER. There are other forces affecting the system, namely those point attractor dynamics that draw the system from the start point to the end point. So we need to account for those.
In [ ]:
# For the discrete system
alpha = 400
beta = 100
start = heart[-1]
end = heart[-1] # change this and run!
forces = dd_heart - (alpha * (beta * (end - heart) - d_heart))
# plot the desired accelerations and forces to apply to
# achieve the desired accelerations
plt.subplot(2, 1, 1)
plt.plot(dd_heart[:, 0], alpha=.5)
plt.gca().set_prop_cycle(None)
plt.plot(forces[:, 0])
plt.legend(['desired acceleration', 'forces to apply'])
plt.title('Forces to apply')
plt.subplot(2, 1, 2)
plt.plot(dd_heart[:, 1], alpha=.5)
plt.gca().set_prop_cycle(None)
plt.plot(forces[:,1])
plt.legend(['desired acceleration', 'forces to apply'])
In [ ]:
trajectory = point_attractor_ff(start, end, forces, dt=dt,
alpha=alpha, beta=beta)
plt.plot(heart[:, 0], heart[:, 1], 'r--', lw=3) # plot the target path
plt.plot(trajectory[:, 0], trajectory[:, 1]) # system trajectory in blue
plt.plot(trajectory[:, 0], trajectory[:, 1], 'b.') # add blue dots at time steps
plt.plot(start[0], start[1], 'bx', mew=4) # blue x at start position
plt.plot(end[0], end[1], 'gx', mew=4) # green x at end position
plt.legend(['desired trajectory', 'actual trajectory'])
plt.xlim([-2, 2])
plt.ylim([-2, 2])
Here, the system is starting and ending at the same point in the trajectory, and it traces it out perfectly.
In [ ]:
# For the rhythmic system
alpha = 400
beta = 100
start = end = [0, 0] # change this and run!
forces = dd_heart - (alpha * (beta * (end - heart) - d_heart))
# plot the desired accelerations and forces to apply to
# achieve the desired accelerations
plt.subplot(2, 1, 1)
plt.plot(dd_heart[:, 0], alpha=.5)
plt.gca().set_prop_cycle(None)
plt.plot(forces[:, 0])
plt.legend(['desired acceleration', 'forces to apply'])
plt.title('Forces to apply')
plt.subplot(2, 1, 2)
plt.plot(dd_heart[:, 1], alpha=.5)
plt.gca().set_prop_cycle(None)
plt.plot(forces[:,1])
plt.legend(['desired acceleration', 'forces to apply'])
In [ ]:
num_loops = 3
forces = np.vstack([forces] * num_loops)
trajectory = point_attractor_ff(start, end, forces, dt=dt,
alpha=alpha, beta=beta)
plt.plot(heart[:, 0], heart[:, 1], 'r--', lw=3) # plot the target path
plt.plot(trajectory[:, 0], trajectory[:, 1]) # system trajectory in blue
plt.plot(trajectory[:, 0], trajectory[:, 1], 'b.') # add blue dots at time steps
plt.plot(start[0], start[1], 'bx', mew=4) # blue x at start position
plt.plot(end[0], end[1], 'gx', mew=4) # green x at end position
plt.legend(['desired trajectory', 'actual trajectory'])
plt.xlim([-2, 2])
plt.ylim([-2, 2])
Here, we've started our system out at [0, 0], and you can see that on the first loop through it's not matching the desired trajectory. However, by the second loop through it's converged to the desired path and we're stably tracing out the heart pattern.
So now we've implemented the most basic possible versions of something that resembles DMPs. But it's hopefully enough to get a feel for how these kinds of systems work. Directly we now move on to neural DMPs!
Similar to how we broke down DMPs into point attractors and a forcing function, we will first discuss implementing neural point attractors and the add in a forcing function!
Our point attractor is a second order system (which means that the dynamics are defined in terms of the second derivative, acceleration):
$\ddot{y} = \alpha \; (\beta \; (y^* - y) - \dot{y})$
at each time step the system velocity, $\dot{y}$, and position, $y$, are updated according to
$\dot{y} = \dot{y} + \ddot{y} * dt$
$y = y + \dot{y} * dt$
To make implementation easier on ourselves, we're going to rewrite the point attractor equations as a first order system. So first we define
$\textbf{y} = \left[ \begin{array}{c}y \\ \dot{y} \end{array} \right]$
This lets us rewrite the dynamics as a first order system
$\dot{\textbf{y}} = \left[ \begin{array}{c} \dot{y} \\ \ddot{y} \end{array} \right ] = \begin{array}{c} \dot{y} \\ \alpha \; (\beta \; (y^* - y) - \dot{y}) \end{array} = \left[ \begin{array}{cc}0 & 1 \\ - \alpha \beta & -\beta \end{array} \right ] \textbf{y} + \left[ \begin{array}{c} 0 \\ \alpha\beta \end{array} \right ] y^*$
So the change in $\textbf{y}$ is dependent on two parts:
1) the current state of the system:
$\left[ \begin{array}{cc}0 & 1 \\ - \alpha \beta & -\beta \end{array} \right ] \textbf{y} = \textbf{A} \textbf{y}$
2) the system input:
$\left[ \begin{array}{c} 0 \\ \alpha \beta \end{array} \right ] y^* = \textbf{B} y^*$
To implement this dynamical system in neurons, we first need to set up an ensemble that represents our variables of interest: $y$ and $\dot{y}$.
NOTE: we make it so neurons represent y OR dy, not both this makes it so that the representation of y does not interfere with the representation of dy.
Because we don't need to compute any nonlinear functions of y and dy on outgoing connections this is OK!
DISCUSS: does this make sense? how does this make you feel? maybe drawing some circles will help?
Once the ensemble is created that represents our system state variables we need to implement the desired system dynamics.
The above equations tell us how $\textbf{y}$ changes over time. We broke it down into two parts, the $\textbf{A}$ and $\textbf{B}$ matrices, which operate on the current state and the input signal, respectively.
So Y is representing our system state, and connections into Y will implement the dynamics.
The first part of the dynamics work on the current state of the system, so we can implement these with a recurrent connection on Y.
The second part of the dynamics operates on the input signal, so we create an input signal and project it into Y.
In [ ]:
import nengo
from models import point_attractor
model = point_attractor.generate()
from nengo_gui.ipython import IPythonViz
IPythonViz(model, cfg='point_attractor.viz.cfg')
Alright, great!
So we've now got point attractors implemented in neurons, all that's left is generating the additional forces that we need to move the system along our desired trajectory.
For discrete movements, we can do this by decoding the required forces off of a ramping signal, and for rhythmic movements we can decode the required forces off of an oscillator (so that they are produced over and over and over and ...)
Here we're going to look at the implementation for a rhythmic movement.
In [ ]:
from models import forcing_functions
from models import oscillator
def generate(data_file, net=None, alpha=1000.0, beta=250):
# generate our forcing function
y_des = np.load(data_file)['arr_0'].T
_, force_functions, _ = forcing_functions.generate(
y_des, rhythmic=True, alpha=alpha, beta=beta)
net = nengo.Network(label='Rhythmic NDMP')
with net:
# --------------------- Inputs ------------------------------
net.input = nengo.Node(size_in=2)
# ------------------- Point Attractors ----------------------
x = point_attractor.generate(
n_neurons=500, alpha=alpha, beta=beta)
nengo.Connection(net.input[0], x.input[0], synapse=None)
y = point_attractor.generate(
n_neurons=500, alpha=alpha, beta=beta)
nengo.Connection(net.input[1], y.input[0], synapse=None)
# -------------------- Oscillators --------------------------
kick = nengo.Node(
nengo.utils.functions.piecewise({0: 1, .05: 0}),
label='kick')
osc = oscillator.generate(net, n_neurons=3000, speed=.025)
osc.label = 'oscillator'
nengo.Connection(kick, osc[0])
# connect oscillator to point attractor
nengo.Connection(osc, x.input[1], synapse=None, **force_functions[0])
nengo.Connection(osc, y.input[1], synapse=None, **force_functions[1])
# -------------------- Output -------------------------------
net.output = nengo.Node(size_in=2)
nengo.Connection(x.output, net.output[0], synapse=None)
nengo.Connection(y.output, net.output[1], synapse=None)
return net
model = generate('models/handwriting_trajectories/star.npz')
from nengo_gui.ipython import IPythonViz
IPythonViz(model, cfg='ndmp.viz.cfg')
In [ ]: